/*
 * Copyright (c) 2024 iSOFT INFRASTRUCTURE SOFTWARE CO., LTD.
 * easyAda is licensed under Mulan PubL v2.
 * You can use this software according to the terms and conditions of the Mulan PubL v2.
 * You may obtain a copy of Mulan PubL v2 at:
 *          http://license.coscl.org.cn/MulanPubL-2.0
 * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
 * EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
 * MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
 * See the Mulan PubL v2 for more details.
 */

#include <plat/config.h>
#include "tools/assembler.h"

#define CONTROL_M         0
#define CONTROL_A         1
#define CONTROL_C         2
#define CONTROL_SA        3
#define CONTROL_SA0       4
#define CONTROL_I         12
#define CONTROL_E0E       24
#define CONTROL_EE        25


#define MMU_MAIR_ATTR1        (0x04 << (8 * 1))
#define MMU_MAIR_ATTR2        (0xff << (8 * 2))
#define MMU_MAIR_ATTR3        (0x44 << (8 * 3))

.global _start
.section ".entry", "ax"
_start:
    tlbi vmalle1is
    dcache cisw
    isb

    ldr x0, =boot_start_time
    ldr x2, = CONFIG_EXEC_ADDR
    sub x2, x0, x2
    ldr x3, = CONFIG_LOAD_ADDR
    add x0, x2 , x3
    mrs x1, cntfrq_el0
    str x1, [x0]

    mrs X0, SCTLR_EL1
    ldr x1, =~(1 << CONTROL_M | 1 << CONTROL_C | 1 << CONTROL_I | 1 << CONTROL_SA | 1 << CONTROL_SA0)
    and x0, x0, x1
    isb
    msr SCTLR_EL1, X0

    bl get_idle_stack_x0_to_x3

    ldr x2, =CONFIG_EXEC_ADDR
    sub x2, x1, x2
    ldr x3, =CONFIG_LOAD_ADDR
    add x1, x2 , x3
    msr sp_el1,x1
    mov sp, x1

    ldr x0, =vectors_entry
    ldr x2, =CONFIG_EXEC_ADDR
    sub x2, x0, x2
    ldr x3, =CONFIG_LOAD_ADDR
    add x2, x2, x3
    msr vbar_el1, x2
    bl arm64_elX_to_el1

#ifdef MMU_ENABLE
    ldr x8, =MMU_MAIR_ATTR1 | MMU_MAIR_ATTR2 | MMU_MAIR_ATTR3
    msr mair_el1, x8
    isb

    ldr x0, =ksram
    ldr x1, =ksdev
    ldr x2, =boot_end
    bl set_mmu

    mrs X2, SCTLR_EL1
    ldr x3, =(1 << CONTROL_M)
    orr x2, x2, x3
    isb
    msr SCTLR_EL1, X2

    bl get_idle_stack_x0_to_x3
    mov sp,x1
    ldr x0, = mmu_enable
    br X0
mmu_enable:
#endif

    bl relocate_data
    bl init_kernel
#ifdef JUMP_EL0
    bl current_context_sp
    add sp, x0, #8

    mov x0, #0b0000
    msr spsr_el1, x0

    ldr x1, =idlestack
    ldr x2, =CONFIG_STACK_SIZE
    add x1, x1, x2
    ldr x2, =rs_ro_start
    sub x2, x1, x2
    ldr x3, =CONFIG_ROOTSERVER_EXEC_ADDR
    add x1, x2, x3
    msr sp_el0, x1

    ldr x1, = idle
    ldr x2, = rs_ro_start
    sub x2, x1, x2
    ldr x3, = CONFIG_ROOTSERVER_EXEC_ADDR
    add x1, x2 , x3
    msr elr_el1, X1

    isb
    eret
    isb
el0_entry:
#endif

    b .


get_idle_stack_x0_to_x3:
    mrs x0, mpidr_el1
    mov x1, #0xff00
    and x0, x0, x1
    lsr x0, x0, #8

    ldr x1, =tempstack
    ldr x2, =CONFIG_STACK_SIZE
    mul x3, x0, x2
    add x1, x1, x3
    add x1, x1, x2

    mrs x4,spsel
    orr x4,x4,#1
    msr spsel, x4

    ret

.section ".stack"
.global tempstack

tempstack:
#ifdef CONFIG_ENABLE_SMP
    .space (CONFIG_SMP_CORES * CONFIG_STACK_SIZE)
#else
    .space CONFIG_STACK_SIZE
#endif

.section ".idle_stack"
.global idlestack

idlestack:
#ifdef CONFIG_ENABLE_SMP
    .space (CONFIG_SMP_CORES * CONFIG_STACK_SIZE)
#else
    .space CONFIG_STACK_SIZE
#endif

